#include "camera.h"
#include <QtOpenGL>

Camera::Camera()
: angle(60.0f)
{
    Vec3f pos = {0, 0, 0};
    setPosition(pos);
    
    Vec3f dir = {0, 0, -1};
    setDirection(dir);
}

Camera::Camera(Vec3f position)
: angle(60.0f)
{
    setPosition(position);
    
    Vec3f dir = {0, 0, -1};
    setDirection(dir);
}

Camera::Camera(Vec3f position, Vec3f direction, float angle)
: angle(angle)
{
    setPosition(position);
    setDirection(direction);
}

float Camera::getVerticalAngle() const
{
    return RadiansToDegrees(2.0f * atanf(tanf(DegreesToRadians(angle / 2.0f)) * viewSize[1] / viewSize[0]));
}

void Camera::getDirection(Vec3f out)
{
    if (directionDirty)
        tangent(direction);
    SetVec3f(out, direction);
}

void Camera::derivePitchYaw()
{
    // The direction is not always normalized
    Vec3f normDir;
    NormalizeVec3f(normDir, direction);
    
    // Project the normalized direction on the XZ-plane and derive the yaw from it
    Vec3f projXZ;
    SetVec3f(projXZ, normDir[0], 0, normDir[2]);
    float derAngle = RadiansToDegrees(acosf(-projXZ[2]));
    if (projXZ[0] < 0)
        yaw = derAngle;
    else
        yaw = -derAngle;
    
    while (yaw < 0)
        yaw += 360;
    while (yaw > 360)
        yaw -= 360;
    
    // Rotate the normalized vector inside the YZ-plane and derive the pitch
    Mat4f rotY;
    RotateYMat4f(rotY, -yaw);
    Vec3f dirYZ;
    MultiplyMat4fVec3f(dirYZ, rotY, normDir);
    derAngle = RadiansToDegrees(asinf(dirYZ[1]));
    if (dirYZ[2] < 0)
        pitch = derAngle;
    else
        pitch = 180 - derAngle;
    
    while (pitch < 0)
        pitch += 360;
    while (pitch > 360)
        pitch -= 360;
}

float Camera::getPitch()
{
    if (pitchYawDirty) {
        derivePitchYaw();
        pitchYawDirty = false;
    }
    
    return pitch;
}

float Camera::getYaw()
{
    if (pitchYawDirty) {
        derivePitchYaw();
        pitchYawDirty = false;
    }
    
    return yaw;
}

float Camera::getFrustumRight()
{
    float zNear = 0.1;
    float zFar = 1000000;
    float right = zNear * tan(DegreesToRadians(angle / 2.0f));
	return right;
}

float Camera::getFrustumLeft()
{
	return -getFrustumRight();
}

float Camera::getFrustumTop()
{
    float zNear = 0.1;
    float zFar = 1000000;
    float right = zNear * tan(DegreesToRadians(angle / 2.0f));
    float top = right * viewSize[1] / viewSize[0];
	return top;
}

float Camera::getFrustumBottom()
{
	return -getFrustumTop();
}

float Camera::getFrustumNear()
{
	return 0.1f;
}

float Camera::getFrustumFar()
{
	return 1000000.0f;
}

void Camera::cameraToWorldMat4f(Mat4f out)
{
    Mat4f rotX, rotY, trans;
    RotateXMat4f(rotX, getPitch());
    RotateYMat4f(rotY, getYaw());
    TranslateMat4f(trans, position);
    
    Mat4f rotYrotX;
    MultiplyMat4f(rotYrotX, rotY, rotX);
    MultiplyMat4f(out, trans, rotYrotX);
}

void Camera::cameraToWorldDirectionMat4f(Mat4f out)
{
    Mat4f rotX, rotY;
    RotateXMat4f(rotX, getPitch());
    RotateYMat4f(rotY, getYaw());
    MultiplyMat4f(out, rotY, rotX);
}

void Camera::screenToWorldMat4f(Mat4f out)
{
    Mat4f normToWorld, invViewport;
    normalizedSpaceToWorldMat4f(normToWorld);
    InverseViewportMat4f(invViewport, viewSize[0], viewSize[1]);
    MultiplyMat4f(out, normToWorld, invViewport);
}

void Camera::worldToScreenMat4f(Mat4f out)
{
    Mat4f viewport, worldToNorm;
    ViewportMat4f(viewport, viewSize[0], viewSize[1]);
    worldToNormalizedSpaceMat4f(worldToNorm);
    MultiplyMat4f(out, viewport, worldToNorm);
}

void Camera::worldToNormalizedSpaceMat4f(Mat4f out)
{
    Mat4f frustum, rotX, rotY, trans;
    
    float zNear = 0.1;
    float zFar = 1000000;
    float right = zNear * tan(DegreesToRadians(angle / 2.0));
    float top = right * viewSize[1] / viewSize[0];
    FrustumMat4f(frustum, -right, right, -top, top, zNear, zFar);
    InverseRotateXMat4f(rotX, getPitch());
    InverseRotateYMat4f(rotY, getYaw());
    InverseTranslateMat4f(trans, position);
    
    Mat4f rotYtrans, rotXrotYtrans;
    MultiplyMat4f(rotYtrans, rotY, trans);
    MultiplyMat4f(rotXrotYtrans, rotX, rotYtrans);
    MultiplyMat4f(out, frustum, rotXrotYtrans);
}

void Camera::normalizedSpaceToWorldMat4f(Mat4f out)
{
    Mat4f invTrans, invRotY, invRotX, invFrustum;
    
    float zNear = 0.1;
    float zFar = 1000000;
    float right = zNear * tan(DegreesToRadians(angle / 2.0));
    float top = right * viewSize[1] / viewSize[0];
    TranslateMat4f(invTrans, position);
    RotateYMat4f(invRotY, getYaw());
    RotateXMat4f(invRotX, getPitch());
    InverseFrustumMat4f(invFrustum, -right, right, -top, top, zNear, zFar);
    
    Mat4f invRotXinvFrustum, invRotYinvRotXinvFrustum;
    MultiplyMat4f(invRotXinvFrustum, invRotX, invFrustum);
    MultiplyMat4f(invRotYinvRotXinvFrustum, invRotY, invRotXinvFrustum);
    MultiplyMat4f(out, invTrans, invRotYinvRotXinvFrustum);
}

void Camera::screenToNormalizedSpaceMat4f(Mat4f out)
{
	InverseViewportMat4f(out, viewSize[0], viewSize[1]);
}

void Camera::normalizedSpaceToScreenMat4f(Mat4f out)
{
	ViewportMat4f(out, viewSize[0], viewSize[1]);
}

void Camera::tangent(Vec3f out)
{
    Vec3f cameraTangent;
    SetVec3f(cameraTangent, 0, 0, -1);
    Mat4f cameraToWorldDirection;
    cameraToWorldDirectionMat4f(cameraToWorldDirection);
    MultiplyMat4fVec3f(out, cameraToWorldDirection, cameraTangent);
}

void Camera::normal(Vec3f out)
{
    Vec3f cameraNormal;
    SetVec3f(cameraNormal, 0, 1, 0);
    Mat4f cameraToWorldDirection;
    cameraToWorldDirectionMat4f(cameraToWorldDirection);
    MultiplyMat4fVec3f(out, cameraToWorldDirection, cameraNormal);
}

void Camera::binormal(Vec3f out)
{
    Vec3f cameraBinormal;
    SetVec3f(cameraBinormal, 1, 0, 0);
    Mat4f cameraToWorldDirection;
    cameraToWorldDirectionMat4f(cameraToWorldDirection);
    MultiplyMat4fVec3f(out, cameraToWorldDirection, cameraBinormal);
}

void Camera::setRightClipPlane(float x)
{
	rightClipDefined = true;
	rightClipPlane = x;
	
	double equation[] = {-1, 0, 0, x};
	glClipPlane(GL_CLIP_PLANE0, equation);
	glEnable(GL_CLIP_PLANE0);
}

void Camera::setLeftClipPlane(float x)
{
	leftClipDefined = true;
	leftClipPlane = x;
	
	double equation[] = {1, 0, 0, -x};
	glClipPlane(GL_CLIP_PLANE1, equation);
	glEnable(GL_CLIP_PLANE1);
}

void Camera::enableClipPlanes()
{
	if (leftClipDefined)
		glEnable(GL_CLIP_PLANE0);
	
	if (rightClipDefined)
		glEnable(GL_CLIP_PLANE1);
	
	clipPlanesEnabled = true;
}

void Camera::disableClipPlanes()
{
	if (leftClipDefined)
		glDisable(GL_CLIP_PLANE0);
	
	if (rightClipDefined)
		glDisable(GL_CLIP_PLANE1);
	
	clipPlanesEnabled = false;
}

void Camera::clearClipPlanes()
{
	disableClipPlanes();
	leftClipDefined = false;
	rightClipDefined = false;
}

void Camera::setModelView()
{
    glRotatef(-getPitch(), 1, 0, 0);
    glRotatef(-getYaw(), 0, 1, 0);
    glTranslatef(-position[0], -position[1], -position[2]);
}

void Camera::setProjection()
{
    float zNear = 0.1;
    float zFar = 1000000;
    float right = zNear * tan(DegreesToRadians(angle / 2.0f));
    float top = right * viewSize[1] / viewSize[0];
    glFrustum(-right, right, -top, top, zNear, zFar);
}

// To be set near the original model data (in code: modelview, model positioning in world, billboard, model center definition)
void Camera::setBillBoard()
{
    glRotatef(getYaw(), 0, 1, 0);
    glRotatef(getPitch(), 1, 0, 0);
}

void Camera::pointAt(Vec3f target)
{
    Vec3f positionToTarget;
    SubtractVec3f(positionToTarget, target, position);
    NormalizeVec3f(direction, positionToTarget);
    pitchYawDirty = true;
}

void Camera::moveForward(float d)
{
    Vec3f scaledTanget;
    tangent(scaledTanget);
    ScaleVec3f(scaledTanget, scaledTanget, d);
    AddVec3f(position, position, scaledTanget);
}

void Camera::moveUpward(float d)
{
    Vec3f scaledNormal;
    normal(scaledNormal);
    ScaleVec3f(scaledNormal, scaledNormal, d);
    AddVec3f(position, position, scaledNormal);
}

void Camera::moveSideways(float d)
{
    Vec3f scaledBinormal;
    binormal(scaledBinormal);
    ScaleVec3f(scaledBinormal, scaledBinormal, d);
    AddVec3f(position, position, scaledBinormal);
}

void Camera::moveX(float d)
{
	position[0] += d;
}

void Camera::moveY(float d)
{
	position[1] += d;
}

void Camera::moveZ(float d)
{
	position[2] += d;
}

void Camera::getWorldPoint(Vec3f out, Vec2f screenPoint, float depth)
{
    getPickRay(screenPoint[0], screenPoint[1]).getPoint(out, depth);
}

void Camera::getRightPlaneComponents(Vec4f out, Vec3f positionScale, float pitchScale, float yawScale)
{
    Mat4f rotX, rotY;
    RotateXMat4f(rotX, getPitch() * pitchScale);
    RotateYMat4f(rotY, getYaw() * yawScale - angle / 2.0);
    Mat4f rotYrotX;
    MultiplyMat4f(rotYrotX, rotY, rotX);
    
    Vec3f tangentDir = {0, 0, -1}, normalDir = {0, 1, 0};
    Vec3f tangentDirWorld, normalDirWorld;
    MultiplyMat4fVec3f(tangentDirWorld, rotYrotX, tangentDir);
    MultiplyMat4fVec3f(normalDirWorld, rotYrotX, normalDir);
    Vec3f rightPlaneNormal;
    CrossProductVec3f(rightPlaneNormal, normalDirWorld, tangentDirWorld);
    
	Vec3f scaledPosition;
	if (rightClipDefined && clipPlanesEnabled)
		SetVec3f(scaledPosition, rightClipPlane + position[0] * positionScale[0], 0, 0);
	else
		SetVec3f(scaledPosition, position[0] * positionScale[0], position[1] * positionScale[1], position[2] * positionScale[2]);
    float D = -InnerProductVec3f(rightPlaneNormal, scaledPosition);
    SetVec4f(out, rightPlaneNormal[0], rightPlaneNormal[1], rightPlaneNormal[2], D);
}

void Camera::getLeftPlaneComponents(Vec4f out, Vec3f positionScale, float pitchScale, float yawScale)
{
    Mat4f rotX, rotY;
    RotateXMat4f(rotX, getPitch() * pitchScale);
    RotateYMat4f(rotY, getYaw() * yawScale + angle / 2.0);
    Mat4f rotYrotX;
    MultiplyMat4f(rotYrotX, rotY, rotX);
    
    Vec3f tangentDir = {0, 0, -1}, normalDir = {0, 1, 0};
    Vec3f tangentDirWorld, normalDirWorld;
    MultiplyMat4fVec3f(tangentDirWorld, rotYrotX, tangentDir);
    MultiplyMat4fVec3f(normalDirWorld, rotYrotX, normalDir);
    Vec3f leftPlaneNormal;
    CrossProductVec3f(leftPlaneNormal, tangentDirWorld, normalDirWorld);
    
	Vec3f scaledPosition;
	if (leftClipDefined && clipPlanesEnabled)
		SetVec3f(scaledPosition, leftClipPlane + position[0] * positionScale[0], 0, 0);
	else
		SetVec3f(scaledPosition, position[0] * positionScale[0], position[1] * positionScale[1], position[2] * positionScale[2]);
    float D = -InnerProductVec3f(leftPlaneNormal, scaledPosition);
    SetVec4f(out, leftPlaneNormal[0], leftPlaneNormal[1], leftPlaneNormal[2], D);
}

void Camera::getTopPlaneComponents(Vec4f out, Vec3f positionScale, float pitchScale, float yawScale)
{
    Mat4f rotX, rotY;
    RotateXMat4f(rotX, getPitch() * pitchScale + getVerticalAngle() / 2.0);
    RotateYMat4f(rotY, getYaw() * yawScale);
    Mat4f rotYrotX;
    MultiplyMat4f(rotYrotX, rotY, rotX);
    
    Vec3f tangentDir = {0, 0, -1}, binormalDir = {1, 0, 0};
    Vec3f tangentDirWorld, binormalDirWorld;
    MultiplyMat4fVec3f(tangentDirWorld, rotYrotX, tangentDir);
    MultiplyMat4fVec3f(binormalDirWorld, rotYrotX, binormalDir);
    Vec3f topPlaneNormal;
    CrossProductVec3f(topPlaneNormal, tangentDirWorld, binormalDirWorld);
    
    Vec3f scaledPosition = {position[0] * positionScale[0], position[1] * positionScale[1], position[2] * positionScale[2]};
    float D = -InnerProductVec3f(topPlaneNormal, scaledPosition);
    SetVec4f(out, topPlaneNormal[0], topPlaneNormal[1], topPlaneNormal[2], D);
}

void Camera::getBottomPlaneComponents(Vec4f out, Vec3f positionScale, float pitchScale, float yawScale)
{
    Mat4f rotX, rotY;
    RotateXMat4f(rotX, getPitch() * pitchScale - getVerticalAngle() / 2.0);
    RotateYMat4f(rotY, getYaw() * yawScale);
    Mat4f rotYrotX;
    MultiplyMat4f(rotYrotX, rotY, rotX);
    
    Vec3f tangentDir = {0, 0, -1}, binormalDir = {1, 0, 0};
    Vec3f tangentDirWorld, binormalDirWorld;
    MultiplyMat4fVec3f(tangentDirWorld, rotYrotX, tangentDir);
    MultiplyMat4fVec3f(binormalDirWorld, rotYrotX, binormalDir);
    Vec3f bottomPlaneNormal;
    CrossProductVec3f(bottomPlaneNormal, binormalDirWorld, tangentDirWorld);
    
    Vec3f scaledPosition = {position[0] * positionScale[0], position[1] * positionScale[1], position[2] * positionScale[2]};
    float D = -InnerProductVec3f(bottomPlaneNormal, scaledPosition);
    SetVec4f(out, bottomPlaneNormal[0], bottomPlaneNormal[1], bottomPlaneNormal[2], D);
}

Ray Camera::getNormalizedSpacePickRay(Vec3f start, Vec3f end)
{
    Mat4f normalizedSpaceToWorld;
    normalizedSpaceToWorldMat4f(normalizedSpaceToWorld);
    Vec3f worldSpaceStart, worldSpaceEnd;
    MultiplyMat4fVec3f(worldSpaceStart, normalizedSpaceToWorld, start);
    MultiplyMat4fVec3f(worldSpaceEnd, normalizedSpaceToWorld, end);
    
    return Ray(worldSpaceStart, worldSpaceEnd);
}

Ray Camera::getPickRay(float screenX, float screenY, bool flip)
{
    float x = screenX;
    float y = (flip ? getHeight() - screenY : screenY);
    
    Vec3f screenSpaceNear = {x, y, -1};
    Vec3f screenSpaceFar = {x, y, 1};
    
    Mat4f screenToWorld;
    screenToWorldMat4f(screenToWorld);
    Vec3f worldSpaceNear, worldSpaceFar;
    MultiplyMat4fVec3f(worldSpaceNear, screenToWorld, screenSpaceNear);
    MultiplyMat4fVec3f(worldSpaceFar, screenToWorld, screenSpaceFar);
    
    return Ray(worldSpaceNear, worldSpaceFar);
}


// Camera Controller Implementation

CameraController::CameraController() : panAngle(0)
{
	SetVec3f(scaling, 1.0f, 1.0f, 1.0f);
	updateCameraUnpanPosition();
	updateCameraTracePosition();
}

void CameraController::pushAnimation(QTimeLine* anim)
{
	anim->start();
}

void CameraController::popAnimation(QTimeLine* anim)
{
	delete anim;
}

void CameraController::setScaling(float horizontal, float vertical)
{
	unpanCamera();
	updateCameraTracePosition();
	scaling[0] = horizontal;
	scaling[1] = vertical;
	Vec3f position;
	position[0] = cameraTracePosition[0] * scaling[0];
	position[1] = cameraTracePosition[1];
	position[2] = cameraTracePosition[2];
	camera.setPosition(position);
	updateCameraUnpanPosition();
	panCamera();
	emit render();
}

void CameraController::move(float horizontalDir, float verticalDir, float frontDir)
{
	unpanCamera();
	
	// move sideways, upward, forward (inversed) works here too with the unpan handling
	
	camera.moveX(horizontalDir);
	camera.moveY(verticalDir);
	camera.moveZ(frontDir);
	updateCameraUnpanPosition();
	updateCameraTracePosition();
	panCamera();
	emit render();
}

void CameraController::moveLeft(float step)
{
	move(-step);
}

void CameraController::moveRight(float step)
{
	move(step);
}

void CameraController::moveTo(Vec3f pos, bool animate)
{
	if (animate) {
		unpanCamera();
		MoveAnimation* anim = new MoveAnimation(this);
		anim->setEndPosition(pos);
		panCamera();
		pushAnimation(anim);
	}
	else {
		unpanCamera();
		camera.setPosition(pos);
		updateCameraUnpanPosition();
		updateCameraTracePosition();
		panCamera();
		emit render();
	}
}

void CameraController::panTo(float angle, float animate)
{
	if (animate) {
		PanAnimation* anim = new PanAnimation(this);
		anim->setEndAngle(angle);
		pushAnimation(anim);
	}
	else {
		panAngle = angle;
		panCamera();
		emit render();
	}
}

void CameraController::moveToTrace(Vec3f tracePos, bool animate)
{
	Vec3f worldPos;
	SetVec3f(worldPos, tracePos[0] * scaling[0], tracePos[1], tracePos[2]);
	moveTo(worldPos, animate);
}

void CameraController::moveTo(QList<Element*> elements, BoundingBox boundingBox, bool animate, bool useVerticalDisplacement, bool useScaleChange, bool useHorizondalDisplacement)
{
	if (elements.size() == 0)
		return;
	
	// Get the final center position for the required constraints
	Vec3f center;
	boundingBox.getCenter(center);
	
	updateCameraTracePosition();
	if (!useVerticalDisplacement)
		center[1] = cameraTracePosition[1];
	if (!useHorizondalDisplacement)
		center[0] = cameraTracePosition[0];
	
	if (!useScaleChange) {
		center[2] = cameraTracePosition[2];
	}
	else {
		// Derive the depth position matching the vertical size of the BB
		float cameraDepth = -getCamera().getFrustumNear() * boundingBox.height() * 0.5f / getCamera().getFrustumTop();
		if (cameraDepth < getCamera().getFrustumNear())
			cameraDepth = getCamera().getFrustumNear() + 0.001f;
		center[2] = cameraDepth;
		
		// Derive the required scale change for the horizontal size
	}
	
	moveToTrace(center, animate);
}

void CameraController::updateCameraUnpanPosition()
{
	camera.getPosition(cameraUnpanPosition);
}

void CameraController::updateCameraTracePosition()
{
	Vec3f position;
	camera.getPosition(position);
	cameraTracePosition[0] = position[0] / scaling[0];
	cameraTracePosition[1] = position[1];
	cameraTracePosition[2] = position[2];
}

void CameraController::getCameraWorldPosition(Vec3f out)
{
//	SetVec3f(out, cameraTracePosition[0] * scaling[0], cameraTracePosition[1], cameraTracePosition[2]);
	SetVec3f(out, cameraUnpanPosition);
}

void CameraController::unpanCamera()
{
	Vec3f position;
	getCameraWorldPosition(position);
	camera.setPosition(position);
	camera.setYaw(camera.getYaw() - panAngle);
}

void CameraController::panCamera()
{
	Vec3f position;
	getCameraWorldPosition(position);
	
	Vec3f tiltDisp;
	SetVec3f(tiltDisp, 0.0f, 0.0f, position[2]);
	
	Mat4f rotY;
	RotateYMat4f(rotY, panAngle);
	Vec3f tiltedDisp;
	MultiplyMat4fVec3f(tiltedDisp, rotY, tiltDisp);
	
	position[0] += tiltedDisp[0];
	position[1] += tiltedDisp[1];
	position[2] = tiltedDisp[2];
		
	camera.setYaw(panAngle);
	camera.setPosition(position);
}


// Animation Implementations

Animation::Animation(CameraController* cameraController)
: cameraController(cameraController)
{
	setUpdateInterval(1000/60);
	setCurveShape(EaseInOutCurve);
	
	connect(this, SIGNAL(valueChanged(qreal)), this, SLOT(setNormalizedValue(qreal)));
	connect(this, SIGNAL(finished()), this, SLOT(setFinished()));
}

void Animation::setNormalizedValue(qreal value)
{
}

void Animation::setFinished()
{
	cameraController->popAnimation(this);
}


MoveAnimation::MoveAnimation(CameraController* cameraController)
: Animation(cameraController)
{
	cameraController->getCamera().getPosition(begin);
	SetVec3f(end, 0, 0, 0);
}

void MoveAnimation::setPositions(Vec3f begin, Vec3f end)
{
	SetVec3f(this->begin, begin);
	SetVec3f(this->end, end);
//	setDuration(DistanceVec3f(this->begin, this->end) * 2.0f);
}

void MoveAnimation::setEndPosition(Vec3f end)
{
	Vec3f cameraBegin;
	cameraController->getCamera().getPosition(cameraBegin);
	setPositions(cameraBegin, end);
}

void MoveAnimation::setNormalizedValue(qreal value)
{
	Vec3f disp;
	SubtractVec3f(disp, end, begin);
	ScaleVec3f(disp, disp, value);
	
	Vec3f newPos;
	AddVec3f(newPos, begin, disp);
	
	cameraController->moveTo(newPos, false);
}


PanAnimation::PanAnimation(CameraController* cameraController)
: Animation(cameraController), clockwise(false)
{
	begin = cameraController->getCamera().getYaw();
	end = 0.0f;
}

void PanAnimation::setAngles(float begin, float end, bool shortest)
{
	this->begin = begin;
	this->end = end;
	
	if (shortest) {
		while (this->begin < 0.0f)
			this->begin += 360.0f;
		while (this->begin >= 360.0f)
			this->begin -= 360.0f;
		while (this->end < 0.0f)
			this->end += 360.0f;
		while (this->end >= 360.0f)
			this->end -= 360.0f;
		
		clockwise = abs(this->end - this->begin) > 180.0f;
	}
}

void PanAnimation::setEndAngle(float end, bool shortest)
{
	setAngles(cameraController->getCamera().getYaw(), end, shortest);
}

void PanAnimation::setNormalizedValue(qreal value)
{
	float disp = end - begin;
	if (clockwise)
		disp += (disp > 0 ? -360.0f : 360.0f);
	float angle = begin + value * disp;
	cameraController->panTo(angle, false);
}
